/**
  ******************************************************************************
  * File Name          : jaka_moveit_driver.cpp
  * Description       : jaka_moveit_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/


#include <ros/ros.h>
#include <ros/package.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <sensor_msgs/JointState.h>

#include <iostream>
#include <string>
#include <cstring>
#include <array>
#include <unistd.h>
#include <cmath>
#include "JAKAZuRobot.h"
#include <pthread.h>


using namespace std;

class JakaRobotDriver {
protected:
    ros::NodeHandle nh;
    string actionName;
    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as;
    control_msgs::FollowJointTrajectoryFeedback feedback;
    control_msgs::FollowJointTrajectoryResult result;

    ros::Publisher jointPublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
    sensor_msgs::JointState jointState;
    ros::Rate loop_rate{50};

private:
    static const size_t robotAxisNumber = 6;
    JAKAZuRobot robot;
    const string robotName;
    char charBuffer[1024];
    errno_t status = ERR_SUCC;
    JointValue jointValue;
    pthread_spinlock_t lock;


public:
    JakaRobotDriver(string action_name, string robot_Name = "AI_robot") :
            actionName(action_name),
            robotName(robot_Name),
            as(nh, action_name,
               boost::bind(&JakaRobotDriver::executeCB,
                           this, _1), false) {
        pthread_spin_init(&lock,0);
        initAndOpenRobot();
//        robot.set_debug_mode(true);
        as.start();
        robot.get_joint_position(&jointValue);
        nh.getParam("/jaka_moveit_driver/joint_names", jointState.name);
        nh.getParam("/jaka_moveit_driver/joint_position", jointState.position);

        while(ros::ok()){
            pubJointState();
            ros::spinOnce();
            loop_rate.sleep();
        }

//        closeRobot();
    }

    ~JakaRobotDriver(void) {
    }

    void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
        //TODO: 抢占任务处理
//        robot.motion_abort();

        trajectory_msgs::JointTrajectory trajectory = goal->trajectory;
        map<int, int> axis2Index = getAxis2IndexMap(trajectory);

        try {
            JointValue value;
            for (int i = 0; i < trajectory.points.size(); i += trajectory.points.size()/2 + 1) {
                value = jointValue_moveit2hw(trajectory.points[i], axis2Index);
                pthread_spin_lock(&lock);
                robot.joint_move(&value, ABS, false, 2, 1, 100, nullptr);
                pthread_spin_unlock(&lock);
//                sleep(1);
            }
            value = jointValue_moveit2hw(trajectory.points.back(), axis2Index);
            pthread_spin_lock(&lock);
            robot.joint_move(&value, ABS, false, 2, 1, 100, nullptr);
            pthread_spin_unlock(&lock);
//            usleep(100000);
        }
        catch (std::runtime_error e) {
            cout << " ERROR!!! " << e.what() << endl;
        }

        BOOL inPosState = FALSE;
        do {
            loop_rate.sleep();
            robot.is_in_pos(&inPosState);
        } while (inPosState == FALSE);

        ROS_INFO("%d waypoints finished", static_cast<int>(trajectory.points.size()));
        result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
        as.setSucceeded();
    }

    bool initAndOpenRobot() {
        string path = ros::package::getPath("jaka_moveit_driver");
        robot.set_SDK_filepath(const_cast<char *>((path + "/jaka_sdk_log.txt").c_str()));
//    robot.set_error_handler(user_error_handle);

        memset(charBuffer, 0, sizeof(charBuffer) / sizeof(*charBuffer));
        robot.get_sdk_version(charBuffer);
        ROS_INFO("=== Robot SDK version: %s", charBuffer);

        memset(charBuffer, 0, sizeof(charBuffer) / sizeof(*charBuffer));
        robot.get_controller_ip(const_cast<char *>(robotName.c_str()), charBuffer);
        if (charBuffer[0] == 0 || strstr(charBuffer, ";") != 0) {
            ROS_INFO("xxx Can't find the robot named [%s]!", robotName.c_str());
            return false;
        } else {
            ROS_INFO("=== Find the robot named [%s], and it's ip address is: %s", robotName.c_str(), charBuffer);
        }

        do {
            status = robot.login_in(charBuffer);
            printCommandResult(status, "login_in");
        } while (status != ERR_SUCC);

        RobotStatus robotStatus;
        do {
            status = robot.get_robot_status(&robotStatus);
            printCommandResult(status, "get_robot_status");
            if (status != ERR_SUCC) continue;
            if (robotStatus.emergency_stop) {
                cout << "xxx Emergency-Stop button was pressed! Please release the button First!" << endl;
                continue;
            }
            if (!robotStatus.powered_on) {
                status = robot.power_on();
                printCommandResult(status, "power_on");
            }
            if (!robotStatus.enabled) {
                status = robot.enable_robot();
                printCommandResult(status, "enable_robot");
            }
        } while (status != ERR_SUCC || robotStatus.emergency_stop || !robotStatus.powered_on || !robotStatus.enabled);

        int id = -1;
        status = robot.get_tool_id(&id);
        cout << "ID: " << id << endl;
        status = robot.get_user_frame_id(&id);
        cout << "ID: " << id << endl;

        return true;
    }

    void closeRobot() {
        status = robot.disable_robot();
        printCommandResult(status, "disable_robot");

        status = robot.power_off();
        printCommandResult(status, "power_off");

        status = robot.login_out();
        printCommandResult(status, "login_out");
    }

    void pubJointState() {
        jointState.header.stamp = ros::Time::now();
        pthread_spin_lock(&lock);
        robot.get_joint_position(&jointValue);
        pthread_spin_unlock(&lock);
        array<double, robotAxisNumber> moveitValue = jointValue_hw2moveit(jointValue);
        for(int i=0; i < robotAxisNumber; ++i){
            jointState.position[i]=moveitValue[i];
        }
        jointPublisher.publish(jointState);
    }

    map<int, int> getAxis2IndexMap(const trajectory_msgs::JointTrajectory &trajectory) const {
        map<int, int> axis2Index;
        for (int i = 0; i < robotAxisNumber; ++i) {
            if (trajectory.joint_names[i] == "shoulder_pan_joint") axis2Index[0] = i;
            else if (trajectory.joint_names[i] == "shoulder_lift_joint") axis2Index[1] = i;
            else if (trajectory.joint_names[i] == "elbow_joint") axis2Index[2] = i;
            else if (trajectory.joint_names[i] == "wrist_1_joint") axis2Index[3] = i;
            else if (trajectory.joint_names[i] == "wrist_2_joint") axis2Index[4] = i;
            else if (trajectory.joint_names[i] == "wrist_3_joint") axis2Index[5] = i;
        }
        return axis2Index;
    }

    array<double, robotAxisNumber> jointValue_hw2moveit(const JointValue& hwValue) {
        array<double, robotAxisNumber> moveitValue;
        moveitValue[0] = hwValue.jVal[0] - M_PI_2;
        moveitValue[1] = -(hwValue.jVal[1] - M_PI_2);
        moveitValue[2] = -(hwValue.jVal[2]);
        moveitValue[3] = -(hwValue.jVal[3] - M_PI_2);
        moveitValue[4] = hwValue.jVal[4] - M_PI;
        moveitValue[5] = hwValue.jVal[5];
        return moveitValue;
    }

    JointValue jointValue_moveit2hw(const trajectory_msgs::JointTrajectoryPoint &point, const map<int,int> &axis2Index){
        JointValue hwValue;
        hwValue.jVal[0] = point.positions[axis2Index.at(0)] + M_PI_2;
        hwValue.jVal[1] = -point.positions[axis2Index.at(1)] + M_PI_2;
        hwValue.jVal[2] = -point.positions[axis2Index.at(2)];
        hwValue.jVal[3] = -point.positions[axis2Index.at(3)] + M_PI_2;
        hwValue.jVal[4] = point.positions[axis2Index.at(4)] + M_PI;
        hwValue.jVal[5] = point.positions[axis2Index.at(5)];
        return hwValue;
    }




//    void user_error_handle(int error_code) {
//        cout << "xxx Error code: " << error_code << endl;
//        ErrorCode error;
//        robot.get_last_error(&error);
//        cout << "xxx Last error code: " << error.code << ", error message: " << error.message << endl;
//    }

    void printCommandResult(errno_t status, const string &commandString) {
        if (status == ERR_SUCC) {
            ROS_INFO("=== %s Succeed!", commandString.c_str());
        } else {
            ROS_INFO("=== %s Failed!", commandString.c_str());
        }
        sleep(2);
    }
};


int main(int argc, char **argv) {
    ros::init(argc, argv, "jaka_moveit_driver");
    JakaRobotDriver robot("jaka/arm_joint_controller/follow_joint_trajectory", "AI_robot");
//    ros::spin();
    return 0;
}
